import os
import time

import numpy as np

def get_imu_record(path):
    list_files = []
    if os.path.exists(path):
        # 遍历文件夹，找到.npy文件
        for root, ds, fs in os.walk(path):
            for f in fs:
                if f.endswith('.npy'):
                    fullname = os.path.join(root, f)
                    list_files.append(fullname)
        for file in list_files:
            lab = os.path.split(os.path.split(file)[0])[-1]
    else:
        print('this path not exist')
    return list_files


def NORM(x):
    return np.math.sqrt(np.dot(x,x))

def dis_ACC(x, y):
    scale = 0.5
    diff = x - y
    dis = np.dot(x, y) / (NORM(x) * NORM(y) + 1e-8)
    dis = (1 - scale * dis) * NORM(diff)
    return dis


def estimate_dtw(slice,template):
    before=time.perf_counter()
    A=[]
    B=[]
    for (iA,iB) in zip(slice,template):
        A.append(np.array([iA.linear_acceleration.x,iA.linear_acceleration.y,iA.linear_acceleration.z]))
        B.append(np.array([iB.linear_acceleration.x,iB.linear_acceleration.y,iB.linear_acceleration.z]))
    N_A = len(A)
    N_B = len(B)
    D = np.zeros([N_A, N_B])
    D[0, 0] = dis_ACC(A[0], B[0])

    #左边一列
    for i in range(1, N_A):
        D[i, 0] = D[i - 1, 0] + dis_ACC(A[i], B[0])
    #下边一行
    for j in range(1, N_B):
        D[0, j] = D[0, j - 1] + dis_ACC(A[0], B[j])
    #中间部分7
    for i in range(1, N_A):
        for j in range(1, N_B):
            D[i, j] = dis_ACC(A[i], B[j]) + min(D[i - 1, j], D[i, j - 1], D[i - 1, j - 1])
    # 路径回溯Q
    i = N_A - 1
    j = N_B - 1
    count = 0
    d = np.zeros(max(N_A, N_B) * 3)
    path = []
    while True:
        if i > 0 and j > 0:
            path.append((i, j))
            m = min(D[i - 1, j], D[i, j - 1], D[i - 1, j - 1])
            if m == D[i - 1, j - 1]:
                d[count] = D[i, j] - D[i - 1, j - 1]
                i = i - 1
                j = j - 1
                count = count + 1

            elif m == D[i, j - 1]:
                d[count] = D[i, j] - D[i, j - 1]
                j = j - 1
                count = count + 1

            elif m == D[i - 1, j]:
                d[count] = D[i, j] - D[i - 1, j]
                i = i - 1
                count = count + 1

        elif i == 0 and j == 0:
            path.append((i, j))
            d[count] = D[i, j]
            count = count + 1
            break

        elif i == 0:
            path.append((i, j))
            d[count] = D[i, j] - D[i, j - 1]
            j = j - 1
            count = count + 1

        elif j == 0:
            path.append((i, j))
            d[count] = D[i, j] - D[i - 1, j]
            i = i - 1
            count = count + 1

    mean = np.sum(d) / count
    after=time.perf_counter()
    print("DTW costs:"+str(after-before)[0:5])
    return mean
    #return mean, path[::-1], D


def data_replacing(slice, template):
    global last_imu_template
    print("检测到IMU数据异常，开始替换数据")

    # for item,target in zip(template,data_train):
    #     item.linear_acceleration.x=target[0]
    #     item.linear_acceleration.y=target[1]
    #     item.linear_acceleration.z=target[2]
    print("替换完成")
    return template